/**
 * Author(s): xuming.Wong <xuming.Wong@gmail.com>
 */

#include "BoidController.h"

namespace PQClient
{
	// Constants for the flyers.
	const float STARTING_SPEED = 20.0;//200.0
	const float MINIMUM_SPEED = 15.0;//150.0
	const float MAXIMUM_SPEED = 35.0;//350.0
	const float FLOCK_FORMING_DISTANCE = 500.0;
	const float ANGLE_OF_VISION = 90.0;
	const float CATCH_UP_INCREASE = 5.0;//50.0
	const float COLLISION_DISTANCE = 5.0;//50
	const float SLOW_DOWN_DECREASE = 5.0;//50
	const float FLOCKING_RADIUS = 15.0;//150

	const float RANGE_OF_FLOCK_HEADINGS = 60.0;//
	const float DOWNWARD_ANGLE_FROM_CEILING = -22.5;
	const float UPWARD_ANGLE_FROM_COLLISION = 60.0;
	const float LAND_COLLISION_DISTANCE = 500.0;
	const float LAND_COLLISION_AVOIDANCE_ACCURACY = 20.0;
	const float LANDSCAPE_CLEARANCE = 150.0;
	const float SIDE_CLEARANCE_ANGLE = 20.0;
	const float WING_STROKES_PER_SECOND = 1.5;
	const float ACCELERATION_RATE = 2.0;
	const float FINE_SCAN_RATE = 5.0;
	const float MEDIUM_SCAN_RATE = 25.0;
	const float FAST_SCAN_RATE = 50.0;
	const int FINE_SCAN_LIMIT = 150;
	const int MEDIUM_SCAN_LIMIT = 250;


	const int INITIAL_NUM_FLYERS = 10;


	// The size of the simulation area is specified here.
	const float X_LIMIT = 1200.0;
	const float Y_LIMIT = 300.0;
	const float Z_LIMIT = 1200.0;


	const float MAXIMUM_TIMESPAN = 0.3;


	std::vector<BoidController *> BoidController::_boidControllers;


	BoidController::BoidController(Boid* boid): _boid(boid)
	{

		_pos=Ogre::Vector3(612.902,206.419,1524.02);

		_speed = STARTING_SPEED;

		_reqSpeed=0.0f;

		_heading=Ogre::Vector2(0,0);
		_reqHeading=Ogre::Vector2(0,0);

		_boidControllers.push_back(this);
	}

	BoidController::~BoidController( )
	{
	}


	// Set the position of the flyer.
	void BoidController::setPosition( float x, float y, float z )
	{
		_pos=Ogre::Vector3(x,y,z);
		_boid->getMeshObject()->setPosition(_pos);

		//throw std::exception("not implemented!");
	}

	// Set the required velocity of the flyer.
	void BoidController::setReqVelocity( float sp, float horiz, float vert )
	{
		// Set the required velocity components.
		_reqSpeed = sp;
		_reqHeading.x = horiz;
		_reqHeading.y = vert;
	}


	// Set the speed of the flyer.
	void BoidController::setReqSpeed( float sp )
	{
		// Set the flyer's required speed.
		_reqSpeed = sp;
	}


	// Retrieve the size of the boid & also update the boid's size data member.
	float BoidController::getSize( )
	{
		float size=0;
		//_boid->getMeshObject()->getBoundingBoxSize().x;
		throw std::exception("not impl!");
		return size;
	}

	// Function that directs a boid to another boid in it's field of vision.
	void BoidController::directToBoid( )
	{
		Ogre::Vector3 offsetVec;
		
		bool newVelocity = false;
		Ogre::Real distance;
		Ogre::Real shortestDistance = FLOCK_FORMING_DISTANCE;

		Ogre::Real hHeadingTo, vHeadingTo;
		Ogre::Real newSpeed, newHHeading, newVHeading;

		int numFlyers =_boidControllers.size();
		

		// Iterate through all the boids ignoring the current one.
		for ( int index = 0; index < numFlyers; index++ ){
			// Skip the current boid.
			if ( _boidControllers[ index ] != this ){
				// Shift the other boid so that the current boid is on the origin.
				// This prevents the screen wrap around interfering.
				offsetVec = _boidControllers[index]->_pos-_pos;

				//wrapAround( &tempX, &tempY, &tempZ );

				// Do a distance check on the boids.
				distance=offsetVec.length();

				if ( distance <= FLOCK_FORMING_DISTANCE ){
					// Check if the boid is in the angle of vision.
					// Find the headings to the other boid.
					// First find the vertical heading.
					if ( distance < 0.5f )  // Prevent division by zero.
					{
						distance =0.5f;
					}

					vHeadingTo = Ogre::Math::ASin( offsetVec.y / distance ).valueDegrees();


					// Then find the horizontal heading.
					// Prevent division by zero.
					if ( ( offsetVec.z > -0.5f ) && ( offsetVec.z < 0.5f ) )
					{
						offsetVec.z=0.5f;
						
					}

					hHeadingTo =  Ogre::Math::ATan( offsetVec.x / offsetVec.z ).valueDegrees();


					// Correct the headings if z is less than zero.
					if ( ( offsetVec.x > 0.0 ) && ( offsetVec.z  < 0.0 ) ){
						hHeadingTo += 180.0;
					}

					if ( ( offsetVec.x < 0.0 ) && (  offsetVec.z  < 0.0 ) ){
						hHeadingTo -= 180.0;
					}


					// Normalize the headings, so that -
					// - they can be properly compared.
					hHeadingTo -= _heading.x;
					vHeadingTo -= _heading.y;
					maintainHeadings( &hHeadingTo, &vHeadingTo );

					if ( ( hHeadingTo <= ANGLE_OF_VISION )
						&& ( hHeadingTo >= -ANGLE_OF_VISION)
						&& ( vHeadingTo <= ANGLE_OF_VISION )
						&& ( vHeadingTo >= -ANGLE_OF_VISION ) )
					{
						if ( distance < shortestDistance ){
							// Restore the headings so that -
							// - they can be used properly.
							hHeadingTo += _heading.x;
							vHeadingTo += _heading.y;
							maintainHeadings( &hHeadingTo, &vHeadingTo );

							// Set the new required velocity.
							newVelocity = true;
							shortestDistance = distance;
							newHHeading = hHeadingTo;
							newVHeading = vHeadingTo;
							newSpeed = _boidControllers[ index ] ->_speed + CATCH_UP_INCREASE;

							// Limit the speed.
							if ( newSpeed > MAXIMUM_SPEED){
								newSpeed = MAXIMUM_SPEED;
							}
						}
					}
				}
			}
		}

		if ( newVelocity ){
			setReqVelocity( newSpeed, newHHeading, newVHeading );
		}
	}


	// Function to enable a boid to match it's flockmates velocites.
	void BoidController::velocityMatching( )
	{
		Ogre::Vector3 offsetVec;
		bool newVelocity = false;
		float distance;
		float hHeadingOf, vHeadingOf;
		float totalSpeed = 0, totalHHeading = 0, totalVHeading = 0;
		int flockmates = 0;
		float newSpeed, newHHeading, newVHeading;
		int numFlyers =_boidControllers.size();

		// Iterate through all the boids ignoring the current one.
		for ( int index = 0; index < numFlyers; index++ )
		{
			// Skip the current boid.
			if ( _boidControllers[ index ] != this ){
				// Shift the other boid so that the current boid is on the origin.
				// This prevents the screen wrap around from interfering.
				offsetVec = _boidControllers[ index ] ->_pos-_pos;
				
				//wrapAround( &tempX, &tempY, &tempZ );

				// Do a distance check on the boids.
				distance = offsetVec.length();

				// If the distance is in the flocking radius.
				if ( distance <=FLOCKING_RADIUS ){
					// Check that the headings are similar.
					// Get the headings of the other boid.
					hHeadingOf = _boidControllers[ index ] -> _heading.x;
					vHeadingOf = _boidControllers[ index ] -> _heading.y;

					// Normalise the headings, so that -
					// - they can be properly compared.
					hHeadingOf -= _heading.x;
					vHeadingOf -= _heading.y;

					maintainHeadings( &hHeadingOf, &vHeadingOf );

					if ( ( hHeadingOf <= +RANGE_OF_FLOCK_HEADINGS )
						&& ( hHeadingOf >= -RANGE_OF_FLOCK_HEADINGS )
						&& ( vHeadingOf <= +RANGE_OF_FLOCK_HEADINGS )
						&& ( vHeadingOf >= -RANGE_OF_FLOCK_HEADINGS ) )
					{
						// Restore the headings so that they can be used properly.
						hHeadingOf += _heading.x;
						vHeadingOf += _heading.y;
						maintainHeadings( &hHeadingOf, &vHeadingOf );

						// Take the boids velocity into account ( in the flock ).
						// Add the components to the totals -
						// - to find an average later on.
						totalSpeed += _boidControllers[ index ] -> _speed;
						totalHHeading += hHeadingOf;
						totalVHeading += vHeadingOf;
						flockmates++;
						newVelocity = true;
					}
				}
			}
		}

		if ( newVelocity )
		{
			newSpeed = totalSpeed / flockmates;
			newHHeading = totalHHeading / flockmates;
			newVHeading = totalVHeading / flockmates;
			setReqVelocity( newSpeed, newHHeading, newVHeading );
		}
	}


	// Function to detect and prevent any boid collisions.
	void BoidController::collisionAvoidance( )
	{
		Ogre::Vector3 offsetVec;
		
		bool newVelocity = false;
		float distance;
		float shortestDistance = COLLISION_DISTANCE;
		float hHeadingTo, vHeadingTo;
		float newSpeed, newHHeading, newVHeading;

		int numFlyers = _boidControllers.size();
		for ( int index = 0; index < numFlyers; index++ ){
			if ( _boidControllers[index] != this ){
				continue;
			}
			offsetVec = _boidControllers[ index ] -> _pos - _pos;
			distance = offsetVec.length();

			if ( distance <= COLLISION_DISTANCE ){
				if ( distance < 0.5 ){
					distance = 0.5;
				}

				vHeadingTo = Ogre::Math::ASin( offsetVec.y / distance ).valueDegrees();
				if ((offsetVec.z > -0.5) && ( offsetVec.z < 0.5 )){
					offsetVec.z = 0.5;
				}

				hHeadingTo = Ogre::Math::ATan( offsetVec.x / offsetVec.z ).valueDegrees();

				if ( ( offsetVec.x > 0.0) && (offsetVec.z < 0.0 ) ){
					hHeadingTo += 180.0;
				}

				if ( ( offsetVec.x  < 0.0 ) && ( offsetVec.z < 0.0 ) ){
					hHeadingTo -= 180.0;
				}

				if ( distance < shortestDistance ){
					newVelocity = true;
					shortestDistance = distance;

					hHeadingTo -= _heading.x;
					vHeadingTo -= _heading.y;
					maintainHeadings( &hHeadingTo, &vHeadingTo );
					if ( hHeadingTo >= 0.0 )  // Turn left.
					{
						newHHeading = _heading.x + ( hHeadingTo - 90 ) * ( distance / COLLISION_DISTANCE );
					}
					if ( hHeadingTo < 0.0 )  // Turn right.
					{
						newHHeading = _heading.x + ( hHeadingTo + 90 )* ( distance / COLLISION_DISTANCE );
					}
					if ( vHeadingTo >= 0.0 )  // Turn down.
					{
						newVHeading = _heading.y + ( vHeadingTo - 90 )* ( distance / COLLISION_DISTANCE );
					}
					if ( vHeadingTo < 0.0 )  // Turn up.
					{
						newVHeading = _heading.y + ( vHeadingTo + 90 )* ( distance / COLLISION_DISTANCE );
					}

					// Set the new speed.
					newSpeed = _boidControllers[ index ]->_speed - (SLOW_DOWN_DECREASE * ( distance / COLLISION_DISTANCE) );

					// Limit the speed.
					if ( newSpeed < MINIMUM_SPEED ){
						newSpeed = MINIMUM_SPEED;
					}
				}
			}
		}
		

		if ( newVelocity )
		{
			setReqVelocity( newSpeed, newHHeading, newVHeading );
		}
	}


	// Calculate a position relative to a boid and the scene, given the distance and direction.
	Ogre::Vector3 BoidController::calculateOffset( float distance,float hAngle, float vAngle )
	{
		Ogre::Vector3 position;

		// 1. Find the Y component of the points position.
		position.y = distance * Ogre::Math::Sin(vAngle / 180.0 * Ogre::Math::PI );
		position.y += _pos.y;  // Make relative to the boid's position.

		// 2. Find the horizontal distance to points position
		//     ( intermediate calculation ).
		float horizontalDistance = distance * Ogre::Math::Cos( vAngle/180.0 * Ogre::Math::PI);

		// 3. Find the X component of the points position.
		position.x = horizontalDistance * Ogre::Math::Sin( hAngle/180.0 * Ogre::Math::PI);

		position.x += _pos.x;  // Make relative to the boid's position.

		// 4. Find the Z component of the points position.
		position.z = horizontalDistance * Ogre::Math::Cos( hAngle/180.0 * Ogre::Math::PI);
		position.z += _pos.z;  // Make relative to the boid's position.
		
		// 5. Perform a wrap around on the position to ensure it is within range.
		//wrapAround( &position.x, &position.y, &position.z );

		return position;
	}


	// Function to prevent any boid collisions with the landscape.
	void BoidController::landscapeAvoidance( )
	{
		// Continue with the function if the lanscape is present.
		// If the position of the boid is over the height limit.
		if ( _pos.y > Y_LIMIT ){
			// Turn the boid downwards to keep in it's correct area.
			_reqHeading.y = DOWNWARD_ANGLE_FROM_CEILING;
		}

		// Make the boid avoid any collision with the landscape.
		// Find the distance to the nearest collision in the path of the boid.
		Ogre::Vector3 offset;
		bool collisionDetected = false;
		float collisionDistance = 0;

		// Scan ahead of the boid using fine -
		// - increments to catch the jagged landscape.
		for (collisionDistance;((!collisionDetected) &&(collisionDistance <= LAND_COLLISION_DISTANCE)); ){
			// 1. Find the position of the point, in the current direction
			//    of the current boid and also at the given collision
			//    distance away from it.
			offset = calculateOffset( collisionDistance, _heading.x, _heading.y );

			// 2. Check if the position of the point is below the landscape.
			//collisionDetected = isBelowLand(offset.x, offset.y, offset.z );

			// 3. Increment the collision distance proportional -
			// - to the distance from the boid.
			if ( collisionDistance < FINE_SCAN_LIMIT ){
				// Fine scan for short range.
				collisionDistance += FINE_SCAN_RATE;
			}else if ( collisionDistance < MEDIUM_SCAN_LIMIT ){
				// Medium scan for medium range.
				collisionDistance += MEDIUM_SCAN_RATE;
			}else{
				// Fast scan for long range.
				collisionDistance += FAST_SCAN_RATE;
			}
		}

		// 4.  Manouver the boid so that it avoids collision with the landscape if necessary.
		if ( collisionDetected ){
			landCollisionAvoidance( collisionDistance );
		}
	}


	// Calculate the X Z distance from a boid, given the distance and direction.
	Ogre::Vector3 BoidController::calculateClearance( float length, float hAngle )
	{
		Ogre::Vector3 distance;

		// Find the X component of the points distance.
		distance.x = length * Ogre::Math::Sin(hAngle / 180.0 * Ogre::Math::PI );

		distance.y = 0;  // Not used in the X Z plane.

		// Find the Z component of the points distance.
		distance.z = length * Ogre::Math::Cos( hAngle / 180.0 * Ogre::Math::PI );

		return distance;
	}


	// Finds the safe route to take to avoid collision with the landscape.
	void BoidController::landCollisionAvoidance( float distance )
	{
		Ogre::Vector3 offset;
		bool safePath = false;
		// Number of scan increments.
		float accuracy = LAND_COLLISION_AVOIDANCE_ACCURACY;

		// Scan arround the collsion point to find a safe path.
		for ( int factor = 1; factor <= (int)accuracy; factor++ ){
			// Turn the boid left.
			offset = calculateOffset(distance, _heading.x - 60.0/accuracy*factor, _heading.y);
			//safePath = !( isBelowLand(offset.x,offset.y - LANDSCAPE_CLEARANCE, offset.z ) );
			if ( safePath ){
				_reqHeading.x = _heading.x - 60.0 / accuracy *factor - SIDE_CLEARANCE_ANGLE;
				return;
			}

			// Turn the boid right.
			offset = calculateOffset( distance, _heading.x + 60.0 /accuracy * factor, _heading.y );
			//safePath = !( isBelowLand(offset.x,offset.y - LANDSCAPE_CLEARANCE, offset.z ) );

			if ( safePath ){
				_reqHeading.x = _heading.x + 60.0 / accuracy *factor + SIDE_CLEARANCE_ANGLE;
				return;
			}

			// Turn the boid up and left.
			offset = calculateOffset( distance, _heading.x - 45.0 / accuracy *factor, _heading.y + 45.0 / accuracy * factor );
			//safePath = !( isBelowLand(offset.x,offset.y - LANDSCAPE_CLEARANCE, offset.z ) );
			if ( safePath )
			{
				_reqHeading.x = _heading.x - 45.0 / accuracy *factor - SIDE_CLEARANCE_ANGLE;
				_reqHeading.y = _heading.y + 45.0 / accuracy * factor;
				return;
			}

			// Turn the boid up and right.
			offset = calculateOffset( distance, _heading.x + 45.0 / accuracy *
									factor, _heading.y + 45.0 / accuracy * factor );
			//safePath = !( isBelowLand(offset.x,offset.y - LANDSCAPE_CLEARANCE, offset.z ) );
			
			if ( safePath ){
				_reqHeading.x = _heading.x + 45.0 / accuracy *factor + SIDE_CLEARANCE_ANGLE;
				_reqHeading.y = _heading.y + 45.0 / accuracy * factor;
				return;
			}

			// Turn the boid upwards.
			offset = calculateOffset( distance, _heading.x,_heading.y + 60.0 / accuracy * factor );
			//safePath = !( isBelowLand(offset.x,offset.y - LANDSCAPE_CLEARANCE, offset.z ) );

			if ( safePath ){
				_reqHeading.y = _heading.y + 60.0 / accuracy * factor;
				return;
			}
		}

		// If no safe route was found, just turn the boid upwards.
		_reqHeading.y = UPWARD_ANGLE_FROM_COLLISION;
	}


	// This function converts a speed & heading vector to an x, y, z vector.
	void BoidController::convertVector( float *xi, float *yi, float *zi )
	{
		// 1. Calculate the Y velocity component.
		*yi = _speed * Ogre::Math::Sin( _heading.y / 180.0 * Ogre::Math::PI);

		// 2. Calculate the horizontal speed ( intermediate calcuation ).
		float horizontalSpeed =Ogre::Math::Sqrt( _speed*_speed-(*yi)*(*yi));

		// 3. Calculate the Z velocity component.
		*zi = horizontalSpeed * Ogre::Math::Cos( _heading.x / 180.0 * Ogre::Math::PI);

		// 4. Calculate the X velocity component.
		*xi = horizontalSpeed * Ogre::Math::Sin( _heading.x / 180.0 * Ogre::Math::PI);

	}


	// Make sure that the headings are in the correct ranges.
	void BoidController::maintainHeadings( float *horiz, float *vert )
	{
		while( *horiz > 180.0 )
		{
			*horiz -= 360.0;
		}
		while( *horiz < -180.0 )
		{
			*horiz += 360.0;
		}
		while( *vert > 90.0 )
		{
			*vert -= 90.0;
		}
		while( *vert < -90.0 )
		{
			*vert += 90.0;
		}
	}


	// Function to make the object tend towards it's required velocity.
	void BoidController::updateVelocity(double time)
	{
		maintainHeadings( &_reqHeading.x, &_reqHeading.y );

		// Calculate the differences in the velocity values.
		float difSpeed = _reqSpeed - _speed;
		float difHHeading = _reqHeading.x - _heading.x;
		float difVHeading = _reqHeading.y - _heading.y;

		// If the horizontal turn is greater than 180 degrees, turn the other way.
		if ( difHHeading < -180.0 ){
			difHHeading = ( _reqHeading.x + 360.0 ) - _heading.x;
		}

		// Update the boid.This prevents any jumping.
		if ( time <= MAXIMUM_TIMESPAN ){
			// Update the flyer's speed.
			_speed+= difSpeed * ACCELERATION_RATE * time;
			// Apply some rotation to the flyer's orientation.
			_heading.x += difHHeading * ACCELERATION_RATE * time;
			_heading.y += difVHeading * ACCELERATION_RATE * time;
		}


		// Update the flyer's visual orientation.
		float x, y, z;
		convertVector( &x, &y, &z );
		setOrientation(x,y,z);
		
	}


	// Make the flyer re-appear on the other side of it's space.
	void BoidController::wrapAround( float *xPtr, float *yPtr, float *zPtr )
	{
		// Loop around the six sides of the cubed area.
		if ( *xPtr > X_LIMIT ){
			*xPtr = -X_LIMIT;
		}

		if ( *xPtr < -X_LIMIT ){
			*xPtr = X_LIMIT;
		}

		// If the position of the boid is over the height limit.
		if ( *yPtr > Y_LIMIT ){
				// Perform wrap around on the boid.
				*yPtr = -Y_LIMIT;
		}

		// If the position of the boid is under the height limit.
		if ( *yPtr < -Y_LIMIT ){
				// Perform wrap around on the boid.
				*yPtr = Y_LIMIT;
		}

		if ( *zPtr > Z_LIMIT ){
			*zPtr = -Z_LIMIT;
		}

		if ( *zPtr < -Z_LIMIT ){
			*zPtr = Z_LIMIT;
		}
	}


	// Move the flyer while the application is running.
	void BoidController::moveObject(float deltaT )
	{
		// Calculate the time in seconds since the last update.

		// Update the flyer's velocity to it's required velocity.
		updateVelocity( deltaT );

		// Convert the flyer's vector before applying it.
		float xi, yi, zi;
		convertVector( &xi, &yi, &zi );

		// Increment the new position of the boid.
		// This prevents the boid from jumping.
		if ( deltaT <= MAXIMUM_TIMESPAN ){
			_pos.x += xi * deltaT;
			_pos.y += yi * deltaT;
			_pos.z += zi * deltaT;
		}

		//wrapAround( &_pos.x, &_pos.y, &_pos.z );  // Make the screen loop around.

		// Set the new position of the flyer.
		setPosition(_pos.x,_pos.y,_pos.z);
		

		// Animate the boids wings if any.
		// Only if the selected mesh is the animated -
		// - one and the simulation is running.
		if ( deltaT <= MAXIMUM_TIMESPAN ){
			// Get the system time in milliseconds.
			unsigned long milliTime = deltaT*1000 *WING_STROKES_PER_SECOND;
			// Remove the seconds, just keeping the milliseconds.
			milliTime %= 1000;
			// Convert the milliseconds into radians.
			float radians = milliTime / 159.1549431;
			// Calculate the X vector component.
			float xVector = Ogre::Math::Sin( radians );

			// Set the new orientation of the wings.
			updateWing();
		}
	}

	void BoidController::update( float deltaT )
	{
		//update 
		this -> directToBoid();

		this -> velocityMatching( );
		this -> collisionAvoidance( );
		this -> landscapeAvoidance( );


		//real move 
		this -> moveObject(deltaT);
	}

	void BoidController::updateWing()
	{
		//throw std::exception("The method or operation is not implemented.");
	}

	void BoidController::setOrientation( float x, float y, float z )
	{
		Ogre::Quaternion orient(1,x,y,z);
		_boid->getMeshObject()->setOrientation(orient);

		//throw std::exception("The method or operation is not implemented.");
	}
}
